#include "vehicle-sample-v5.h"

namespace vehicle_sample_v5
{
	class VehicleSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(VehicleSample, SessionSampleVars)
		VehicleSampleData data;

		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;
			sample.values() = Array<GeneralElement>(80 + 2 * (data.contour.size() + data.trajectory.size()));

			sample.values()[0] = data.contour.size();
			sample.values()[1] = data.trajectory.size();
			sample.values()[2] = data.speed;
			sample.values()[3] = data.yawRate;
			sample.values()[4] = data.curvature;
			sample.values()[5] = data.accelX;
			sample.values()[6] = data.accelY;
			sample.values()[7] = data.vehicleWidth;
			sample.values()[8] = data.vehicleLength;
			sample.values()[9] = data.vehicleHeight;
			sample.values()[10] = data.steerAngleRatio;
			sample.values()[11] = data.wheelBase;
			sample.values()[12] = data.rearTread;
			sample.values()[13] = data.frontOverhang;
			sample.values()[14] = data.steerAngle;
			sample.values()[15] = data.steerAngleRate;
			sample.values()[16] = data.brake;
			sample.values()[17] = data.throttle;
			sample.values()[18] = (UInt)data.gear;
			sample.values()[19] = (UInt)data.turn;
			sample.values()[20] = data.flSpeed;
			sample.values()[21] = data.frSpeed;
			sample.values()[22] = data.rlSpeed;
			sample.values()[23] = data.rrSpeed;
			sample.values()[24] = data.engineSpeed;
			sample.values()[25] = data.engineTorque;
			sample.values()[26] = (UInt)data.horn;
			sample.values()[27] = (UInt)data.headLight;
			sample.values()[28] = (UInt)data.wiper;
			sample.values()[29] = data.kilometerAge;
			sample.values()[30] = data.frontEndLateralSpeed;
			sample.values()[31] = data.rearEndLateralSpeed;
			sample.values()[32] = data.steerTorque;
			sample.values()[33] = data.jerkX;
			sample.values()[34] = data.jerkY;
			sample.values()[35] = data.maxCurvature;
			sample.values()[36] = data.maxAcceleration;
			sample.values()[37] = data.maxDeceleration;
			sample.values()[38] = data.maxAccelY;
			sample.values()[39] = data.maxSteerAngle;
			sample.values()[40] = data.maxSteerAngleRate;
			sample.values()[41] = data.brakeDeadZone;
			sample.values()[42] = data.throttleDeadZone;
			sample.values()[43] = data.flMass;
			sample.values()[44] = data.frMass;
			sample.values()[45] = data.rlMass;
			sample.values()[46] = data.rrMass;
			sample.values()[47] = data.flAngle;
			sample.values()[48] = data.frAngle;
			sample.values()[49] = data.frontCorneringStiffness;
			sample.values()[50] = data.rearCorneringStiffness;
			sample.values()[51] = data.flLightPosX;
			sample.values()[52] = data.flLightPosY;
			sample.values()[53] = data.flLightPosZ;
			sample.values()[54] = data.rlLightPosX;
			sample.values()[55] = data.rlLightPosY;
			sample.values()[56] = data.rlLightPosZ;
			sample.values()[57] = data.pitchToGround;

			for (UInt i = 0; i < data.contour.size(); i++)
			{
				sample.values()[80 + 2 * i] = data.contour[i].x;
				sample.values()[81 + 2 * i] = data.contour[i].y;
			}

			UInt trajectoryBase = 80 + 2 * data.contour.size();
			for (UInt i = 0; i < data.trajectory.size(); i++)
			{
				sample.values()[trajectoryBase + 2 * i] = data.trajectory[i].x;
				sample.values()[trajectoryBase + 2 * i + 1] = data.trajectory[i].y;
			}

			sample.significantCount() = 80 + 2 * data.contour.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "vehicle-sample-v5" || protocol.startsWith("object-vehicle-sample-v1"))
			{
				if (values.size() < 80) return FALSE;

				UInt contourSize = (UInt)values[0].value;
				UInt trajectorySize = (UInt)values[1].value;
				if (values.size() == 80 + 2 * contourSize) trajectorySize = 0;
				else if (values.size() != 80 + 2 * (contourSize + trajectorySize)) return FALSE;

				data.speed = values[2].valid ? values[2].value : 0;
				data.yawRate = values[3];
				data.curvature = values[4];
				data.accelX = values[5];
				data.accelY = values[6];
				data.vehicleWidth = values[7].valid ? values[7].value : 1.9;
				data.vehicleLength = values[8].valid ? values[8].value : 4.6;
				data.vehicleHeight = values[9].valid ? values[9].value : 1.5;
				data.steerAngleRatio = values[10].valid ? values[10].value : 15;
				data.wheelBase = values[11].valid ? values[11].value : 2.8;
				data.rearTread = values[12].valid ? values[12].value : 1.6;
				data.frontOverhang = values[13].valid ? values[13].value : 0.9;
				data.steerAngle = values[14];
				data.steerAngleRate = values[15];
				data.brake = values[16];
				data.throttle = values[17];
				data.gear = values[18].valid ? (GearPosition)(UInt)values[18].value : GearPosition::Unknown;
				data.turn = values[19].valid ? (TurnState)(UInt)values[19].value : TurnState::Unknown;
				data.flSpeed = values[20];
				data.frSpeed = values[21];
				data.rlSpeed = values[22];
				data.rrSpeed = values[23];
				data.engineSpeed = values[24];
				data.engineTorque = values[25];
				data.horn = values[26].valid ? (Horn)(UInt)values[26].value : Horn::Unknown;
				data.headLight = values[27].valid ? (HeadLight)(UInt)values[27].value : HeadLight::Unknown;
				data.wiper = values[28].valid ? (Wiper)(UInt)values[28].value : Wiper::Unknown;
				data.kilometerAge = values[29].value;
				data.frontEndLateralSpeed = values[30];
				data.rearEndLateralSpeed = values[31];
				data.steerTorque = values[32];
				data.jerkX = values[33];
				data.jerkY = values[34];
				data.maxCurvature = values[35];
				data.maxAcceleration = values[36];
				data.maxDeceleration = values[37];
				data.maxAccelY = values[38];
				data.maxSteerAngle = values[39];
				data.maxSteerAngleRate = values[40];
				data.brakeDeadZone = values[41];
				data.throttleDeadZone = values[42];
				data.flMass = values[43];
				data.frMass = values[44];
				data.rlMass = values[45];
				data.rrMass = values[46];
				data.flAngle = values[47];
				data.frAngle = values[48];
				data.frontCorneringStiffness = values[49];
				data.rearCorneringStiffness = values[50];
				data.flLightPosX = values[51];
				data.flLightPosY = values[52];
				data.flLightPosZ = values[53];
				data.rlLightPosX = values[54];
				data.rlLightPosY = values[55];
				data.rlLightPosZ = values[56];
				data.pitchToGround = values[57];

				data.contour = Array<Point2D>(contourSize);
				for (UInt i = 0; i < contourSize; i++)
				{
					data.contour[i].x = (Float)values[80 + 2 * i].value;
					data.contour[i].y = (Float)values[81 + 2 * i].value;
				}

				data.trajectory = Array<Point2D>(trajectorySize);
				UInt trajectoryBase = 80 + 2 * contourSize;
				for (UInt i = 0; i < trajectorySize; i++)
				{
					data.trajectory[i].x = (Float)values[trajectoryBase + 2 * i].value;
					data.trajectory[i].y = (Float)values[trajectoryBase + 2 * i + 1].value;
				}

				return TRUE;
			}
			else if (protocol == "vehicle-sample-v4")
			{
				if (values.size() < 40) return FALSE;

				UInt contourSize = (UInt)values[0].value;
				UInt trajectorySize = (UInt)values[1].value;
				if (values.size() == 40 + 2 * contourSize) trajectorySize = 0;
				else if (values.size() != 40 + 2 * (contourSize + trajectorySize)) return FALSE;

				data.speed = values[2].valid ? values[2].value : 0;
				data.yawRate = values[3];
				data.curvature = values[4];
				data.accelX = values[5];
				data.accelY = values[6];
				data.vehicleWidth = values[7].valid ? values[7].value : 1.9;
				data.vehicleLength = values[8].valid ? values[8].value : 4.6;
				data.vehicleHeight = values[9].valid ? values[9].value : 1.5;
				data.steerAngleRatio = values[10].valid ? values[10].value : 15;
				data.wheelBase = values[11].valid ? values[11].value : 2.8;
				data.rearTread = values[12].valid ? values[12].value : 1.6;
				data.frontOverhang = values[13].valid ? values[13].value : 0.9;
				data.steerAngle = values[14];
				data.steerAngleRate = values[15];
				data.brake = values[16];
				data.throttle = values[17];
				data.gear = values[18].valid ? (GearPosition)(UInt)values[18].value : GearPosition::Unknown;
				data.turn = values[19].valid ? (TurnState)(UInt)values[19].value : TurnState::Unknown;
				data.flSpeed = values[20];
				data.frSpeed = values[21];
				data.rlSpeed = values[22];
				data.rrSpeed = values[23];
				data.engineSpeed = values[24];
				data.engineTorque = values[25];
				data.horn = values[26].valid ? (Horn)(UInt)values[26].value : Horn::Unknown;
				data.headLight = values[27].valid ? (HeadLight)(UInt)values[27].value : HeadLight::Unknown;
				data.wiper = values[28].valid ? (Wiper)(UInt)values[28].value : Wiper::Unknown;
				data.kilometerAge = values[29].value;
				data.frontEndLateralSpeed = values[30];
				data.rearEndLateralSpeed = values[31];
				data.steerTorque = values[32];
				data.jerkX = values[33];
				data.jerkY = values[34];

				data.contour = Array<Point2D>(contourSize);
				for (UInt i = 0; i < contourSize; i++)
				{
					data.contour[i].x = (Float)values[40 + 2 * i].value;
					data.contour[i].y = (Float)values[41 + 2 * i].value;
				}

				data.trajectory = Array<Point2D>(trajectorySize);
				UInt trajectoryBase = 40 + 2 * contourSize;
				for (UInt i = 0; i < trajectorySize; i++)
				{
					data.trajectory[i].x = (Float)values[trajectoryBase + 2 * i].value;
					data.trajectory[i].y = (Float)values[trajectoryBase + 2 * i + 1].value;
				}

				return TRUE;
			}
			else if (protocol == "vehicle-sample-v3")
			{
				if (values.size() < 27) return FALSE;

				UInt trajectorySize = (UInt)values[0].value;
				if (values.size() == 27) trajectorySize = 0;
				else if (values.size() != 27 + 2 * trajectorySize) return FALSE;

				data.speed = values[1].valid ? values[1].value : 0;
				data.yawRate = values[2];
				data.curvature = values[3];
				data.accelX = values[4];
				data.accelY = values[5];
				data.vehicleWidth = values[6].valid ? values[6].value : 1.9;
				data.vehicleLength = values[7].valid ? values[7].value : 4.6;
				data.steerAngleRatio = values[8].valid ? values[8].value : 15;
				data.wheelBase = values[9].valid ? values[9].value : 2.8;
				data.rearTread = values[10].valid ? values[10].value : 1.6;
				data.frontOverhang = values[11].valid ? values[11].value : 0.9;
				data.steerAngle = values[12];
				data.brake = values[13];
				data.throttle = values[14];
				data.gear = values[15].valid ? (GearPosition)(UInt)values[15].value : GearPosition::Unknown;
				data.turn = values[16].valid ? (TurnState)(UInt)values[16].value : TurnState::Unknown;
				data.flSpeed = values[17];
				data.frSpeed = values[18];
				data.rlSpeed = values[19];
				data.rrSpeed = values[20];
				data.engineSpeed = values[21];
				data.engineTorque = values[22];
				data.horn = values[23].valid ? (Horn)(UInt)values[23].value : Horn::Unknown;
				data.headLight = values[24].valid ? (HeadLight)(UInt)values[24].value : HeadLight::Unknown;
				data.wiper = values[25].valid ? (Wiper)(UInt)values[25].value : Wiper::Unknown;
				data.kilometerAge = values[26].value;

				data.trajectory = Array<Point2D>(trajectorySize);
				for (UInt i = 0; i < trajectorySize; i++)
				{
					data.trajectory[i].x = (Float)values[27 + 2 * i].value;
					data.trajectory[i].y = (Float)values[27 + 2 * i + 1].value;
				}

				return TRUE;
			}

			return FALSE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<VehicleSample>() || !ss2.is<VehicleSample>()) return SessionSample();

			VehicleSampleData& s1 = ss1.as<VehicleSample>().getVars()->data;
			VehicleSampleData& s2 = ss2.as<VehicleSample>().getVars()->data;

			VehicleSample vs;
			vs.timestamp() = timestamp;
			
			VehicleSampleData& so = vs.getVars()->data;
			so.speed = s1.speed * w1 + s2.speed * w2;
			so.kilometerAge = s1.kilometerAge * w1 + s2.kilometerAge * w2;
			so.yawRate = s1.yawRate * w1 + s2.yawRate * w2;
			so.curvature = s1.curvature * w1 + s2.curvature * w2;
			so.accelX = s1.accelX * w1 + s2.accelX * w2;
			so.accelY = s1.accelY * w1 + s2.accelY * w2;
			so.jerkX = s1.jerkX * w1 + s2.jerkX * w2;
			so.jerkY = s1.jerkY * w1 + s2.jerkY * w2;
			so.vehicleWidth = s1.vehicleWidth;
			so.vehicleLength = s1.vehicleLength;
			so.vehicleHeight = s1.vehicleHeight;
			so.frontOverhang = s1.frontOverhang;
			so.rearTread = s1.rearTread;
			so.steerAngleRatio = s1.steerAngleRatio;
			so.wheelBase = s1.wheelBase;
			so.steerAngle = s1.steerAngle * w1 + s2.steerAngle * w2;
			so.steerAngleRate = s1.steerAngleRate * w1 + s2.steerAngleRate * w2;
			so.steerTorque = s1.steerTorque * w1 + s2.steerTorque * w2;
			so.brake = s1.brake * w1 + s2.brake * w2;
			so.throttle = s1.throttle * w1 + s2.throttle * w2;
			so.gear = w1 > w2 ? s1.gear : s2.gear;
			so.turn = w1 > w2 ? s1.turn : s2.turn;
			so.horn = w1 > w2 ? s1.horn : s2.horn;
			so.headLight = w1 > w2 ? s1.headLight : s2.headLight;
			so.wiper = w1 > w2 ? s1.wiper : s2.wiper;
			so.flSpeed = s1.flSpeed * w1 + s2.flSpeed * w2;
			so.frSpeed = s1.frSpeed * w1 + s2.frSpeed * w2;
			so.rlSpeed = s1.rlSpeed * w1 + s2.rlSpeed * w2;
			so.rrSpeed = s1.rrSpeed * w1 + s2.rrSpeed * w2;
			so.engineSpeed = s1.engineSpeed * w1 + s2.engineSpeed * w2;
			so.engineTorque = s1.engineTorque * w1 + s2.engineTorque * w2;
			so.frontEndLateralSpeed = s1.frontEndLateralSpeed * w1 + s2.frontEndLateralSpeed * w2;
			so.rearEndLateralSpeed = s1.rearEndLateralSpeed * w1 + s2.rearEndLateralSpeed * w2;
			so.trajectory = w1 > w2 ? s1.trajectory : s2.trajectory;
			so.contour = s1.contour;
			so.maxCurvature = s1.maxCurvature;
			so.maxAcceleration = s1.maxAcceleration;
			so.maxDeceleration = s1.maxDeceleration;
			so.maxAccelY = s1.maxAccelY;
			so.maxSteerAngle = s1.maxSteerAngle;
			so.maxSteerAngleRate = s1.maxSteerAngleRate;
			so.brakeDeadZone = s1.brakeDeadZone;
			so.throttleDeadZone = s1.throttleDeadZone;
			so.flMass = s1.flMass;
			so.frMass = s1.frMass;
			so.rlMass = s1.rlMass;
			so.rrMass = s1.rrMass;
			so.flAngle = s1.flAngle * w1 + s2.flAngle * w2;
			so.frAngle = s1.frAngle * w1 + s2.frAngle * w2;
			so.frontCorneringStiffness = s1.frontCorneringStiffness;
			so.rearCorneringStiffness = s1.rearCorneringStiffness;
			so.flLightPosX = w1 > w2 ? s1.flLightPosX : s2.flLightPosX;
			so.flLightPosY = w1 > w2 ? s1.flLightPosY : s2.flLightPosY;
			so.flLightPosZ = w1 > w2 ? s1.flLightPosZ : s2.flLightPosZ;
			so.rlLightPosX = w1 > w2 ? s1.rlLightPosX : s2.rlLightPosX;
			so.rlLightPosY = w1 > w2 ? s1.rlLightPosY : s2.rlLightPosY;
			so.rlLightPosZ = w1 > w2 ? s1.rlLightPosZ : s2.rlLightPosZ;
			so.pitchToGround = s1.pitchToGround * w1 + s2.pitchToGround * w2;

			return vs.as<SessionSample>();
		}
	};
}

using namespace vehicle_sample_v5;

VehicleSample::VehicleSample() : Object<VehicleSampleVars>(new VehicleSampleVars, TRUE)
{}

FullTimestamp& VehicleSample::timestamp()
{
	return vars->timestamp;
}

VehicleSampleData *VehicleSample::operator ->()
{
    return &vars->data;
}

String VehicleSample::getSubjectProtocolName()
{
    return "vehicle-sample-v5";
}

String VehicleSample::getObjectProtocolName(UInt channel)
{
    if (channel == UINF) return "object-vehicle-sample-v1";
    else return (String)"object-vehicle-sample-v1@" + channel;
}

SessionGeneralSample VehicleSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> VehicleSample::getSubjectProtocolNames()
{
	return Array<String>::create(3, "vehicle-sample-v3", "vehicle-sample-v4", "vehicle-sample-v5");
}

Array<String> VehicleSample::getObjectProtocolNames(UInt channel)
{
    if (channel == UINF) return Array<String>::create(1, "object-vehicle-sample-v1");
    else
	{
		Array<String> names(1);
		names[0] = (String)"object-vehicle-sample-v1@" + channel;
		return names;
	}
}

Bool VehicleSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}